/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <math.h>
#include "Devices.h"



void drawText(float x, float y, void *font, const char *string)
{
  const char *c;
  glRasterPos2f(x,y);
  for (c=string; *c != '\0'; c++)
  {
    glutBitmapCharacter(font, *c);
  }
}

using namespace MaCIGUI;

/**** CPositionLayer ****/

CPositionLayer::CPositionLayer(std::string Name, CMachineUnit* unit, 
                              MaCI::Position::CPositionClient* positionClient)
  :CDeviceLayer(Name, unit,positionClient),
   positionX(0),
   positionY(0),
   positionZ(0),
   heading(0),
   iconMode(ICON_MODE_J2B2),
   iPositionSelected(false),
   iHeadingSelected(false),
   iInitX(0.0),
   iInitY(0.0),
   iInitXEnd(0.0),
   iInitYEnd(0.0),
   seq(-1),
   enableBreadCrumbs(false)
{
  Enabled=true;
  bcMutex=ownMutex_Init();
}

CPositionLayer::~CPositionLayer()
{
  dPrint(1," ");
}

void CPositionLayer::Draw(float viewLeft, float viewRight, float viewTop, 
                          float viewBottom, float scale)//GLuint renderMode, float alpha)
{
  //dPrint(1,"draw");
  glPushAttrib(GL_ALL_ATTRIB_BITS);

  glMatrixMode(GL_MODELVIEW);

  glPushMatrix();

  if (MachineUnit->Selected)
  {
    glColor3f(UNIT_COLOR_SELECTED);
    glLineWidth(LINE_WIDTH_SELECTED);
  } else {
    glColor3f(UNIT_COLOR_DESELECTED);
    glLineWidth(LINE_WIDTH_DESELECTED);
  }

  //draw robot
  glBegin(GL_POINTS);
    glVertex2f(positionX,positionY);
  glEnd();

  float step=2*M_PI/32;

  GLTransform();
  if (iconMode==ICON_MODE_J2B2)
  {
    glBegin(GL_LINE_LOOP);
      glVertex2f(0.25,0);
      glVertex2f(0.05,-0.1);
      glVertex2f(0.05,0.1);
      for (float a=0;a<2*M_PI;a+=step)
      {
        glVertex2f(cos(a)*0.25,sin(a)*0.25);
      }
    glEnd();
  } else if (iconMode==ICON_MODE_AVANT) {
    glBegin(GL_LINE_LOOP);
      glVertex2f(-0.5,0.5);
      glVertex2f(-0.5,-0.5);
      glVertex2f(2,-0.5);
      glVertex2f(2,0);
      glVertex2f(1.7,-0.1);
      glVertex2f(1.7,0.1);
      glVertex2f(2,0);
      glVertex2f(2,0.5);

    glEnd();
  }
  //TODO: add printing of unit name and position next to unit symbol
  
  glPopMatrix();

  drawText(positionX-0.3,positionY+0.3,GLUT_BITMAP_HELVETICA_12, MachineUnit->groupName.c_str());
  
  glPushMatrix();
  if(iPositionSelected){
    if(iHeadingSelected){

      glPointSize(5);
      glBegin(GL_POINTS);
      glVertex2f(iInitX,iInitY);
      glEnd();

      glBegin(GL_LINE_STRIP);
      glVertex2f(iInitX,iInitY);
      glVertex2f(iInitXEnd,iInitYEnd);
      glEnd();
    }else{

      glPointSize(5);
      glBegin(GL_POINTS);
      glVertex2f(iInitX,iInitY);
      glEnd();
    }

  }
  
  if (enableBreadCrumbs)
  {
    
    ownMutex_Lock(bcMutex);
    
    std::list<posWTime>::iterator bci;
    glPointSize(2);
    glBegin(GL_POINTS);
    for (bci=breadCrumbs.begin(); bci!=breadCrumbs.end(); bci++)
    {
      glVertex2f(bci->position.GetPose2D()->x , bci->position.GetPose2D()->y);
    }
    glEnd();
    
    ownMutex_Unlock(bcMutex);
    
  }
  
  glPopMatrix();
  glPopAttrib();

}

void CPositionLayer::OnClick(float x, float y)
{
  //dPrint(1,"OnClick");

  if(selectionOnDevice){
    dPrint(10,"OnClick %f,%f, positionSelected before %d",x,y, iPositionSelected);
    if(!iPositionSelected || iHeadingSelected){
      iInitX = x;
      iInitY = y;
      iPositionSelected = true;
      iHeadingSelected = false;
    }else{

      iInitXEnd = x;
      iInitYEnd = y;
      iHeadingSelected = true;
    }
  }
}

void CPositionLayer::OnKey(int keyCode)
{
  if (keyCode=='A')
  {
    if (iconMode==ICON_MODE_AVANT)
      iconMode=ICON_MODE_J2B2;
    else
      iconMode=ICON_MODE_AVANT;
  }else if(keyCode == WXK_RETURN){
    if(iPositionSelected && iHeadingSelected){
      MaCI::Position::CPositionClient *pc=(MaCI::Position::CPositionClient*)MaCIClient;
      
      float initHeading = atan2(iInitYEnd-iInitY,iInitXEnd-iInitX); ///< calculate heading
      float l = sqrt((iInitXEnd-iInitX)*(iInitXEnd-iInitX) + (iInitYEnd-iInitY)*(iInitYEnd-iInitY));
      
      //Construct the localization-struct
      MaCI::Position::TPose2D loc(iInitX,iInitY,initHeading);
      MaCI::Position::TVariance2D var(l,l,M_PI/4);
      MaCI::Position::TProbability prob(1);
      dPrint(1,"Setting position %f %f %f, with variance %f %f %f", iInitX,iInitY, initHeading,var.x,var.y,var.a);
      //      ASRobo::Drivers::Devices::Localize::TLocalization2D loc(initX,initY,initHeading,l,l,M_PI/4);
      pc->SetPosition(loc,var,prob);
      iPositionSelected = false;
      iHeadingSelected = false;
    }
    


  }else if(keyCode == 'I'){
    MaCI::Position::CPositionClient *pc=(MaCI::Position::CPositionClient*)MaCIClient;
    pc->SetInitialize();
  } else if (keyCode == 'B') {
    enableBreadCrumbs=!enableBreadCrumbs;
  }
}

void CPositionLayer::OnKeyUp(int keyCode)
{
}

void CPositionLayer::OnKillFocus()
{
  
}

//update position data
void CPositionLayer::Update(int timeout)
{
  
  MaCI::Position::CPositionClient *pc=(MaCI::Position::CPositionClient*)MaCIClient;
  
  MaCI::Position::CPositionData pos;
  
  ownMutex_Lock(bcMutex);
  
  if (pc!=NULL)
  {
    if (pc->GetPositionEvent(pos,&seq,timeout))
    {
      
      if (pos.GetPose2D())
      {
        heading=pos.GetPose2D()->a;
        positionX=pos.GetPose2D()->x;
        positionY=pos.GetPose2D()->y;
        
        if (enableBreadCrumbs)
        {
          posWTime pwt;
          pwt.position=pos;
          MaCI::Common::TTimestamp t;
          t.SetToCurrentTime();
          pwt.time=t;
          breadCrumbs.push_back(pwt);
        }
      } else {
        dPrint(1,"Position was NULL!");
      }
    }
  }
  
  if (enableBreadCrumbs)
  {
    //delete breadcrumbs that are too old
    gim::time t(true);
    t.reduceTimeMs(BREADCRUMB_LIFETIME_S*1000);
  
    while (breadCrumbs.size())
    {
      //if too old remove
      if (breadCrumbs.front().time.GetGimTime() < t)
      {
        breadCrumbs.pop_front();
      } else {
        break;
      }
    }
  }
  
  ownMutex_Unlock(bcMutex);
  
}

void CPositionLayer::GLTransform()
{
  glTranslatef(positionX,positionY,0);
  glRotatef(heading*(180.0/M_PI),0,0,1);
}

float CPositionLayer::GetX()
{
  return positionX;
}

float CPositionLayer::GetY()
{
  return positionY;
}



/**** CRangingLayer  ****/

CRangingLayer::CRangingLayer(std::string Name, CMachineUnit* unit, MaCI::Ranging::CRangingClient* rangingClient)
:CDeviceLayer(Name,unit,rangingClient),
 seq(0)
{
  Enabled=true; //it is logical to show ranging data as default
  distArrayMutex=ownMutex_Init();
}

CRangingLayer::~CRangingLayer()
{
  
}

void CRangingLayer::Draw(float viewLeft, float viewRight, float viewTop, float viewBottom, float scale)//GLuint renderMode, float alpha);
{
  glPushMatrix();
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  ownMutex_Lock(distArrayMutex);
  int c=distArray.size();
  float d,h;

  glColor3f(1.0,0.0,0.0);

  refLayer->GLTransform();

  glPointSize(2);
  glBegin(GL_POINTS);

  for (int i=0;i<c;i++)
  {
    d=distArray.at(i).distance;
    if(d >0){
      h=distArray.at(i).angle;
      glVertex2f(cos(h)*d,sin(h)*d);
    }
  }

  glEnd();

  ownMutex_Unlock(distArrayMutex);
  glPopMatrix();
  glPopAttrib();
}

void CRangingLayer::OnClick(float x, float y)
{

};

void CRangingLayer::OnKey(int keyCode)
{

}

void CRangingLayer::OnKeyUp(int keyCode)
{
}

void CRangingLayer::OnKillFocus()
{
  
}

void CRangingLayer::Update(int timeout)
{
  MaCI::Ranging::CRangingClient *rc=(MaCI::Ranging::CRangingClient*)MaCIClient;
  ownMutex_Lock(distArrayMutex);
  if (rc->GetDistanceArray(distArray,NULL,NULL,&seq, timeout ) )
  {

  }else{
    //ownSleep_ms(10); //TODO: why this?
  }
  ownMutex_Unlock(distArrayMutex);
}

void CRangingLayer::GLTransform()
{

}

float CRangingLayer::GetX()
{
  return 0;
}

float CRangingLayer::GetY()
{
  return 0;
}



/*** SpeedCtrl Layer ***/

const float CSpeedCtrlLayer::SPEED_STEP=0.1;
const float CSpeedCtrlLayer::ANGULAR_SPEED_STEP=0.5;
const float CSpeedCtrlLayer::ACCELERATION=0.2;

CSpeedCtrlLayer::CSpeedCtrlLayer(std::string Name, CMachineUnit* unit, 
                  MaCI::SpeedCtrl::CSpeedCtrlClient* speedCtrlClient)
:CDeviceLayer(Name,unit,speedCtrlClient),
speed(0),
angularSpeed(0),
multiplier(1)
{
  Enabled=true;
}

CSpeedCtrlLayer::~CSpeedCtrlLayer()
{
  
}

void CSpeedCtrlLayer::Draw(float viewLeft, float viewRight, float viewTop, 
                          float viewBottom, float scale)
{
  
}

void CSpeedCtrlLayer::OnClick(float x, float y)
{
  
}

void CSpeedCtrlLayer::OnKey(int keyCode)
{
  
  switch (keyCode)
  {
    case WXK_UP:
      speed=SPEED_STEP*multiplier;
      break;
    case WXK_DOWN:
      speed=-SPEED_STEP*multiplier;
      break;
    case WXK_LEFT:
      angularSpeed=ANGULAR_SPEED_STEP;
      break;
    case WXK_RIGHT:
      angularSpeed=-ANGULAR_SPEED_STEP;
      break;
    case WXK_ADD:
    case WXK_NUMPAD_ADD:
      multiplier++;
      break;
    case WXK_SUBTRACT:
    case WXK_NUMPAD_SUBTRACT:
      if (multiplier > 1)
        multiplier--;
      break;
  }
  
}

void CSpeedCtrlLayer::OnKeyUp(int keyCode)
{
  switch (keyCode)
  {
    case WXK_UP:
      speed=0;
      break;
    case WXK_DOWN:
      speed=0;
      break;
    case WXK_LEFT:
      angularSpeed=0;
      break;
    case WXK_RIGHT:
      angularSpeed=0;
  }
  ((MaCI::SpeedCtrl::CSpeedCtrlClient*)MaCIClient)->SetSpeed(speed, angularSpeed, ACCELERATION);
}

void CSpeedCtrlLayer::OnKillFocus()
{
  dPrint(1,"Lost focus");
  speed=0;
  angularSpeed=0;
  ((MaCI::SpeedCtrl::CSpeedCtrlClient*)MaCIClient)->SetSpeed(speed, angularSpeed, ACCELERATION);
}

void CSpeedCtrlLayer::Update(int timeout)
{
  if (speed || angularSpeed)
  {
    ((MaCI::SpeedCtrl::CSpeedCtrlClient*)MaCIClient)->SetSpeed(speed, angularSpeed, ACCELERATION);
  }
}

void CSpeedCtrlLayer::GLTransform()
{
  
}

float CSpeedCtrlLayer::GetX()
{
  return 0;
}

float CSpeedCtrlLayer::GetY()
{
  return 0;
}


/*** CoordinateDrive Layer ***/

using namespace MaCI::CoordinateDrive;
CCoordinateDriveLayer::CCoordinateDriveLayer(std::string Name, CMachineUnit* unit, 
					    CCoordinateDriveClient* coordinateDriveClient):
  CDeviceLayer(Name,unit,coordinateDriveClient),
  currentlyOnWaitPoint(false),
  drivingPoints(),
  tempPointForAngleMode(),
  angleModePointSet(false),
  drivePointsMutex(),
  waitPointWanted(false),
  pathModePointWanted(false),
  angleModePointWanted(false),
  drawAngleModePoint(false),
  printInfos(true),
  printHelp(true),
  latestPointSeq(0),
  coordinateSeq(-1),
  modeTurnFirstToGoal(),
  modeUseAngle(),
  modePathDriving(),
  modeUseReverse()
{
  Enabled=true;
  drivePointsMutex=ownMutex_Init();
  

}

CCoordinateDriveLayer::~CCoordinateDriveLayer()
{
  ownMutex_Destroy(drivePointsMutex);
}

/*void CCoordinateDriveLayer::drawText(float x, float y, void *font, char *string)
{
  char *c;
  glRasterPos2f(x,y);
  for (c=string; *c != '\0'; c++)
  {
    glutBitmapCharacter(font, *c);
  }
}*/

void CCoordinateDriveLayer::Draw(float viewLeft, float viewRight, float viewTop, float viewBottom, 
	  float scale)
{

  std::vector<TCoordinate>::iterator i;
  std::vector<TCoordinate>::reverse_iterator ri;

  //Print info
  if(printInfos && selectionOnDevice){
    //   dPrint(1,"scale %f, viewLeft %f",scale,viewLeft);
    PrintInfos(viewLeft,viewRight,viewTop, viewBottom, scale);

  }
  if(printHelp && selectionOnDevice){
    //   dPrint(1,"scale %f, viewLeft %f",scale,viewLeft);
    PrintHelp(viewLeft,viewRight,viewTop, viewBottom, scale);

  }
  //Print text if we are on wait point
  if(currentlyOnWaitPoint){
    // dPrint(1,"CURRENTLYONWAITPOINT");
    glColor3f(1.0,0.0,0.0);
    std::string cont ="WAITING FOR CONTINUE!";
    drawText(latestPoint.x,latestPoint.y, GLUT_BITMAP_TIMES_ROMAN_24, (char *)(cont.c_str()));
    cont ="Press 'c' for continue";
    drawText(latestPoint.x,latestPoint.y-24/scale, GLUT_BITMAP_TIMES_ROMAN_24, (char *)(cont.c_str()));
  }


  glPushAttrib(GL_ALL_ATTRIB_BITS);
  //Draw the drivingPoints
  if(!modePathDriving)
    glColor3f(0.5,0.0,0.5);
  else
    glColor3f(0.0,0.0,1.0);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glBegin(GL_LINE_STRIP);
  ownMutex_Lock(drivePointsMutex);
  //Draw the driving points
  for (ri=drivingPoints.rbegin();ri!=drivingPoints.rend();ri++)
    {
    glVertex2f(ri->x,ri->y);
  }
  //if pathmode draw a line from the last to the first point
  if(!drivingPoints.empty() && drivingPoints[0].isPathTarget)
    glVertex2f(drivingPoints.rbegin()->x, drivingPoints.rbegin()->y);
  glEnd();

  refLayer->GLTransform();
  glVertex2f(0,0);

  glPopMatrix();

  glPointSize(5);
  glBegin(GL_POINTS);

  for (i=drivingPoints.begin();i!=drivingPoints.end();i++)
  {
    glColor3f(0.5,0.0,0.5);

    if(i->isWaitPoint && modePathDriving)
      glColor3f(1.0,0.0,0.0);

    glVertex2f(i->x,i->y);
  }
  
  glEnd();

  if(modeUseAngle && !drivingPoints.empty()){
    for (ri=drivingPoints.rbegin();ri!=drivingPoints.rend();ri++){
      float xEnd = ri->x + cos(ri->a) * 0.2;
      float yEnd = ri->y + sin(ri->a) * 0.2;;
      glBegin(GL_LINE_STRIP); 
      glColor3f(1.0,0.0,0.0);
      glVertex2f(ri->x,ri->y);
      glVertex2f(xEnd,yEnd);
      glEnd();
    }

  }
  //Draw the selected points

  glColor3f(0.5,0.5,0.5);
  
  
  glBegin(GL_LINE_STRIP);
  for (ri=selectedPoints.rbegin();ri!=selectedPoints.rend();ri++){
    glVertex2f(ri->x,ri->y);
  }
  //if pathmode draw a line from the last to the first point
  if(pathModePointWanted && !selectedPoints.empty())
    glVertex2f(selectedPoints.rbegin()->x, selectedPoints.rbegin()->y);

  refLayer->GLTransform();



  glEnd();
  glPointSize(5);
  glBegin(GL_POINTS);

  for (i=selectedPoints.begin();i!=selectedPoints.end();i++){
    if(i->isWaitPoint && pathModePointWanted)
      glColor3f(0.4,0.0,0.0);
    else
      glColor3f(0.5,0.5,0.5);
    
      glVertex2f(i->x,i->y);
   

  }

  if(drawAngleModePoint){
    glColor3f(0.5,0.5,0.5);
    glVertex2f(tempPointForAngleMode.x,tempPointForAngleMode.y);
  }
  glPushMatrix();
  glEnd();
  //
  if(angleModePointWanted){
    for (ri=selectedPoints.rbegin();ri!=selectedPoints.rend();ri++){
      float xEnd = ri->x + cos(ri->a) * 0.2;
      float yEnd = ri->y + sin(ri->a) * 0.2;;
      glBegin(GL_LINE_STRIP); 
      glColor3f(1.0,0.0,0.0);
      glVertex2f(ri->x,ri->y);
      glVertex2f(xEnd,yEnd);
      glEnd();
    }

  }
  ownMutex_Unlock(drivePointsMutex);



  glPopAttrib();

}

void CCoordinateDriveLayer::PrintHelp(float viewLeft, float viewRight, float viewTop, 
				      float viewBottom, float scale)
{
  int lineSize= 12;

  float startingPointText= viewRight - 22*lineSize/scale;
  float startingPointButton = viewRight -6*lineSize/scale;
  std::vector<std::string> functions;
  std::vector<std::string> buttons;


  functions.push_back("Emergency stop");
  buttons.push_back("Esc");
  functions.push_back("");
  buttons.push_back("");

  functions.push_back("Send and start driving toward");
  buttons.push_back("Enter");

  functions.push_back("coordinates");
  buttons.push_back("");

  functions.push_back("Remove last point");
  buttons.push_back("Backspace");

  functions.push_back("Remove all points");
  buttons.push_back("Delete");
  functions.push_back("");
  buttons.push_back("");

  functions.push_back("Path/normal coordinateDrive");
  buttons.push_back("Shift");

  functions.push_back("Wait point to Path");
  buttons.push_back("s");

  functions.push_back("Continue path driving");
  buttons.push_back("c");

  functions.push_back("Set angle to point");
  buttons.push_back("a");

  functions.push_back("Set CoordinateDrives modes on/of");
  buttons.push_back("1,2,3,4");
  functions.push_back("");
  buttons.push_back("");
  functions.push_back("CoordinateDrive information on/off");
  buttons.push_back("i");

  functions.push_back("Help on/off");
  buttons.push_back("h");

  //Topic
  glColor3f(0.7,0.7,0.7);
  drawText(startingPointButton,viewTop-lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)(buttons[0].c_str()));
  drawText(startingPointText,viewTop-lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)(functions[0].c_str()));


  //Information
  glColor3f(1.0,1.0,1.0);
  for(int i = 1; i < (int)functions.size();i++){

    drawText(startingPointButton,viewTop-(i+1)*lineSize/scale, GLUT_BITMAP_HELVETICA_10, (char *)(buttons[i].c_str()));
    drawText(startingPointText,viewTop-(i+1)*lineSize/scale, GLUT_BITMAP_HELVETICA_10, (char *)(functions[i].c_str()));
  }
}

void CCoordinateDriveLayer::PrintInfos(float viewLeft, float viewRight, float viewTop, 
				       float viewBottom, float scale)
{
  int lineSize= 12;
  std::string path = "Path driving";
  std::string turn = "Turn toward goal";
  std::string angle = "Turn to wanted angle at goal";
  std::string reverse = "Use reverse";
  std::string topic = "CoordinateDrive modes @ robot:";
 
  std::string topic2 ="Selection modes:";
  std::string path2 = "Path target point:";
  std::string angle2 = "Angle to target point:";
  glColor3f(0.7,0.7,0.7);
  drawText(viewLeft+20*lineSize/scale,viewTop-lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)(topic.c_str()));

  if(modePathDriving)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft+20*lineSize/scale, viewTop-2*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)path.c_str());

  if(modeTurnFirstToGoal)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft+20*lineSize/scale, viewTop-3*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)turn.c_str());

  if(modeUseAngle)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft+20*lineSize/scale, viewTop-4*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)angle.c_str());

  if(modeUseReverse)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft+20*lineSize/scale, viewTop-5*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)reverse.c_str());

  glColor3f(0.7,0.7,0.7);
  drawText(viewLeft,viewTop-lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)(topic2.c_str()));

  if(pathModePointWanted)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft, viewTop-2*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)path2.c_str());

  if(angleModePointWanted)
    glColor3f(0.0,1.0,0.0);
  else
    glColor3f(1.0,0.0,0.0);
  drawText(viewLeft, viewTop-3*lineSize/scale, GLUT_BITMAP_TIMES_ROMAN_10, (char *)angle.c_str());


}

void CCoordinateDriveLayer::OnClick(float x, float y)
{
  if(selectionOnDevice){
    if(!angleModePointWanted){
      TCoordinate c(x,y,0.0,waitPointWanted);
      ownMutex_Lock(drivePointsMutex);
      selectedPoints.push_back(c);
      drawAngleModePoint = false;
      ownMutex_Unlock(drivePointsMutex);
    }else{
      if(angleModePointSet){
     
	ownMutex_Lock(drivePointsMutex);
	float angle = atan2((y-tempPointForAngleMode.y),(x-tempPointForAngleMode.x));
	TCoordinate c(tempPointForAngleMode.x,tempPointForAngleMode.y,angle ,waitPointWanted);
	selectedPoints.push_back(c);
	angleModePointSet = false;
	drawAngleModePoint = false;
	ownMutex_Unlock(drivePointsMutex);
      }else{
	dPrint(1,"Setting temp point");
	ownMutex_Lock(drivePointsMutex);
	tempPointForAngleMode.x = x;
	tempPointForAngleMode.y = y;
	tempPointForAngleMode.a = 0.0;
	tempPointForAngleMode.isWaitPoint = waitPointWanted;
	angleModePointSet = true;
	drawAngleModePoint = true;
	ownMutex_Unlock(drivePointsMutex);
      }
    }
  }
}
void CCoordinateDriveLayer::OnKey(int keyCode)
{
  if(selectionOnDevice){

   CCoordinateDriveClient *cClient =
      (CCoordinateDriveClient *)MaCIClient;

    switch (keyCode){

      //EMERGENCY STOP
    case WXK_ESCAPE:
      cClient->SetStop();
      break;

      //Remove last point
    case WXK_BACK:
      ownMutex_Lock(drivePointsMutex);
      if(!modeUseAngle)
      if (!selectedPoints.empty())
	selectedPoints.pop_back();
      ownMutex_Unlock(drivePointsMutex);
      break;

      //Path mode
    case WXK_SHIFT:
      pathModePointWanted = !pathModePointWanted;
      break;

      //Remove driving points
    case WXK_DELETE:
      ownMutex_Lock(drivePointsMutex);
      if(!selectedPoints.empty()){
	selectedPoints.clear();
      }else{
	drivingPoints.clear();
	currentlyOnWaitPoint = false;
	cClient->RemoveCoordinates();
      }
      drawAngleModePoint = false;
      angleModePointSet = false;
      ownMutex_Unlock(drivePointsMutex);
      break;

      //Send points
    case WXK_RETURN:
    
      if(!selectedPoints.empty()){
	cClient->RemoveCoordinates();
	for(unsigned int i= 0; i < selectedPoints.size(); i++){
	  cClient->SetCoordinate(selectedPoints[i].x,
				 selectedPoints[i].y,
				 selectedPoints[i].a,
				 false,
				 pathModePointWanted,
				 selectedPoints[i].isWaitPoint);
	}
	selectedPoints.clear();
	cClient->SetStart();
	currentlyOnWaitPoint = false;
      }
      break;
    case 'A':
      angleModePointWanted = !angleModePointWanted;
      break;
    case 'S':
      if(!selectedPoints.empty()){
	selectedPoints.back().isWaitPoint = !selectedPoints.back().isWaitPoint;
      }
      break;
    //continue
    case 'C':
      currentlyOnWaitPoint = false;
      cClient->Continue();
      break;

    case '1':
      ownMutex_Lock(drivePointsMutex);
      if(modePathDriving){
	cClient->UnsetDriveMode(KModePathDriving);
	
      }else{
	cClient->SetDriveMode(KModePathDriving);

      }
      ownMutex_Unlock(drivePointsMutex);
      break;

    case '2':
      if(modeTurnFirstToGoal){
	cClient->UnsetDriveMode(KModeTurnFirstTowardGoal);

      }else{
	cClient->SetDriveMode(KModeTurnFirstTowardGoal);

      }

      break;
      
    case '3':
      if(modeUseAngle){
	cClient->UnsetDriveMode(KModeUseAngle);

      }else{
	cClient->SetDriveMode(KModeUseAngle);

      }
      break;

    case '4':
      if(modeUseReverse){
	cClient->UnsetDriveMode(KModeUseReverse);
      }else{
	cClient->SetDriveMode(KModeUseReverse);
      }
      break;
  
    case 'I':
      printInfos = !printInfos;
      break;
    case 'H':
      printHelp = !printHelp;
      break;
    default:
      dPrint(10,"No implementation for keycode : %c",keyCode);
      break;
    }

  }
}
void CCoordinateDriveLayer::OnKeyUp(int keyCode)
{

}
void CCoordinateDriveLayer::OnKillFocus()
{

}
    
void CCoordinateDriveLayer::Update(int timeout)
{

  CCoordinateDriveClient *cClient =
    (CCoordinateDriveClient *)MaCIClient;
  TCoordinate2D coord;
  TCoordinateArray cArray;
  TDriveModeArray dArray;
  MaCI::Common::TTimestamp tstamp;

  ownMutex_Lock(drivePointsMutex);

  if(cClient->GetCoordinates(cArray,tstamp,NULL,timeout)){
    drivingPoints.clear();
    for(unsigned int i = 0; i < cArray.size(); i++){
      drivingPoints.push_back(TCoordinate(cArray[i].x,
					  cArray[i].y,
					  cArray[i].a,
					  cArray[i].isWaitPoint,
					  cArray[i].isPathTarget));

    }
  }
  if(cClient->GetOpenDrivingModes(dArray,timeout)){
    modeTurnFirstToGoal = false;
    modeUseAngle = false;
    modePathDriving = false;
    modeUseReverse = false;
    for(unsigned int i = 0; i < dArray.size(); i++){
      switch(dArray[i].mode){
      case KModePathDriving:
        modePathDriving = true;
        break;
      case KModeTurnFirstTowardGoal:
        modeTurnFirstToGoal = true;
        break;
      case KModeUseAngle:
        modeUseAngle = true;
        break;
      case KModeUseReverse:
        modeUseReverse = true;
        break;
      }
    }
  }

  if(cClient->GetLatestPointReached(coord,tstamp,&latestPointSeq,timeout)){
    dPrint(1,"Got latest point, %d, wait %d",latestPointSeq,coord.isWaitPoint);
    latestPoint.x = coord.x;
    latestPoint.y = coord.y;
    if(coord.isWaitPoint == 1){
      dPrint(1,"Currently on waitPoint");
      currentlyOnWaitPoint = true;
    }
  }
 

  ownMutex_Unlock(drivePointsMutex);
}

void CCoordinateDriveLayer::GLTransform()
{

}
float CCoordinateDriveLayer::GetX()
{
  return 0.0;
}
float CCoordinateDriveLayer::GetY()
{
  return 0.0;
}


/***   Map Layer   ***/

CMapLayer::CMapLayer(std::string Name, CMachineUnit* unit, 
                  MaCI::Map::CMapClient* mapClient)
:CDeviceLayer(Name, unit,mapClient),
seq(-1),
updated(false),
inited(false),
displayList2D(0),
drawTreshold(0.4)
{
  Enabled=true;
  z=-1;
}

CMapLayer::~CMapLayer()
{
  
}

void CMapLayer::Draw(float viewLeft, float viewRight, float viewTop, 
    float viewBottom, float scale)
{
  glPushAttrib(GL_ALL_ATTRIB_BITS);

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  
  if (displayList2D && !updated)
  {
    glCallList(displayList2D);
  } else if (updated || mapper.GetMapLines()->size()) {
    //Create display list
    /*if (displayList2D)
    {
      dPrint(1,"deleting list");
      glDeleteLists(displayList2D,1);
    }*/
    if (!displayList2D)
      displayList2D=glGenLists(1);
    
    //dPrint(1,"creating list");
    glNewList(displayList2D,GL_COMPILE_AND_EXECUTE);
      Lock();
      
      //std::vector<MaCI::Map::CMapData*>::iterator iter;
      gmbMap::const_iterator iter;
      //for (iter=mapData.begin();iter!=mapData.end();iter++)
      
     
      
      for (iter=mapper.GetGroundModel()->begin();iter!=mapper.GetGroundModel()->end();iter++)
      {
        //float reso=(*iter)->GetMapHeader()->resolution;
        float reso=mapper.GetResolution();
        //dPrint(1,"reso=%f",reso);
        
        /*
        for (int i=0; i<(*iter)->GetHeightMapBlockCount(); i++)
        {  
          if ((*iter)->GetHeightMapBlockHeader(i)==NULL)
          {
            dPrint(1,"Block is NULL!")
            break;
          }
          int samples_x=lrint(1.0*(*iter)->GetHeightMapBlockHeader(i)->width/reso+1);
          int samples_y=lrint(1.0*(*iter)->GetHeightMapBlockHeader(i)->height/reso+1);
          float pos_x=(*iter)->GetHeightMapBlockHeader(i)->x;
          float pos_y=(*iter)->GetHeightMapBlockHeader(i)->y;
          int or_x=(*iter)->GetHeightMapBlockHeader(i)->dataOriginX;
          int or_y=(*iter)->GetHeightMapBlockHeader(i)->dataOriginY;
          float* data=(float*)(*iter)->GetHeightMapBlockData(i);
          /*dPrint(1,"block: x=%f y=%f w=%f h=%f sx=%d sy=%d ox=%d oy=%d reso=%f",pos_x, 
              pos_y, (*iter)->GetHeightMapBlockHeader(i)->width,
              (*iter)->GetHeightMapBlockHeader(i)->height, samples_x, samples_y,
              or_x, or_y, reso);
          */
          int samples_x=GROUND_MODEL_BLOCK_SIZE;
          int samples_y=GROUND_MODEL_BLOCK_SIZE;
          
          float pos_x=iter->second->x;
          float pos_y=-iter->second->y;
          
          //glPointSize(3);
          glBegin(GL_QUADS);
          //glBegin(GL_POINTS);
          for (int x=0;x<samples_x-1;x+=1)
          {
            for (int y=0;y<samples_y-1;y+=1)
            {
              float h=iter->second->data[y*samples_x+x];
              //dPrint(1,"h=%f",h)
              if (h==UNMAPPED_AREA_VALUE)
              {
                //glColor3f(0,0,0.2);
                //glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
                /*glVertex3f(pos_x+(x+1)*reso,pos_y+y*reso,z);
                glVertex3f(pos_x+(x+1)*reso,pos_y+(y+1)*reso,z); 
                glVertex3f(pos_x+x*reso,pos_y+(y+1)*reso,z);*/
                
              } else {
                //if (h>=0.5)
                //{
                  glColor3f(h,1.0-h,0);
                  glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
                //}
                h=iter->second->data[y*samples_x+x+1];
                glColor3f(h,1-h,0);
                glVertex3f(pos_x+(x+1)*reso,pos_y-y*reso,z);
                
                h=iter->second->data[(y+1)*samples_x+x+1];
                glColor3f(h,1-h,0);
                glVertex3f(pos_x+(x+1)*reso,pos_y-(y+1)*reso,z);
  
                h=iter->second->data[(y+1)*samples_x+x];
                glColor3f(h,1-h,0);
                glVertex3f(pos_x+x*reso,pos_y-(y+1)*reso,z);
              }
              /*float h=data[y*(samples_x-1)+x];
              glColor3f(h,1-h,0);
              glVertex3f((x-or_x)*reso+pos_x,(y-or_y)*reso+pos_y,z);
              
              h=data[y*(samples_x-1)+x+1];
              glColor3f(h,1-h,0);
              glVertex3f((x+1-or_x)*reso+pos_x,(y-or_y)*reso+pos_y,z);
              
              h=data[(y+1)*(samples_x-1)+x+1];
              glColor3f(h,1-h,0);
              glVertex3f((x+1-or_x)*reso+pos_x,(y+1-or_y)*reso+pos_y,z);
              
              h=data[(y+1)*(samples_x-1)+x];
              glColor3f(h,1-h,0);
              glVertex3f((x-or_x)*reso+pos_x,(y+1-or_y)*reso+pos_y,z);
              */              
              
            }
          }
          glEnd();
        //}
      }
      
      occuMap::const_iterator oiter;
      
      for (oiter=mapper.GetOccuModel()->begin();oiter!=mapper.GetOccuModel()->end();oiter++)
      {
        float reso=mapper.GetResolution();
        
        int samples_x=GROUND_MODEL_BLOCK_SIZE;
        int samples_y=GROUND_MODEL_BLOCK_SIZE;
        
        float pos_x=iter->second->x;
        float pos_y=-iter->second->y;
        
        //dPrint(1,"h=%d",(int)(iter->second->data[0]) );
        
        //glPointSize(3);
        glBegin(GL_QUADS);
        //glBegin(GL_POINTS);
        for (int x=0;x<samples_x-1;x+=1)
        {
          for (int y=0;y<samples_y-1;y+=1)
          {
            int hh=oiter->second->data[y*samples_x+x];
            unsigned char h=(unsigned char)hh;
            
            if (hh==UNMAPPED_AREA_VALUE)
            {
              //glColor3f(0,0,0.2);
              //glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
              
            } else {
              //printf("%d ",hh);
              if (h>=drawTreshold*255.0)
              {
                
                //glColor3f(1,1,0);
                glColor3ub(h,0,255-h);
                glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
              //}              
                hh=oiter->second->data[y*samples_x+x+1];
                h=(unsigned char)hh;
                glColor3ub(h,0,255-h);
                glVertex3f(pos_x+(x+1)*reso,pos_y-y*reso,z);
                
                hh=oiter->second->data[(y+1)*samples_x+x+1];
                h=(unsigned char)hh;
                glColor3ub(h,0,255-h);
                glVertex3f(pos_x+(x+1)*reso,pos_y-(y+1)*reso,z);
  
                hh=oiter->second->data[(y+1)*samples_x+x];
                h=(unsigned char)hh;
                glColor3ub(h,0,255-h);
                glVertex3f(pos_x+x*reso,pos_y-(y+1)*reso,z);
              }
            }
          }
          
       
        }
        glEnd();
      }
      
       //draw lines
      glLineWidth(2);
      glColor3f(MAP_LINE_COLOR);
      glBegin(GL_LINES);
      std::vector<MaCI::Map::TMapLine> *lines=mapper.GetMapLines();
      for (std::vector<MaCI::Map::TMapLine>::iterator  i=lines->begin(); i!=lines->end(); i++)
      {
        glVertex3f(i->x1,i->y1,z);
        glVertex3f(i->x2,i->y2,z);
      }
      glEnd();
      
      Unlock();
      
    
    glEndList();
    
    updated=false;
  }
  
  glPopMatrix();
  glPopAttrib();
}

void CMapLayer::OnClick(float x, float y)
{
  
}

void CMapLayer::OnKey(int keyCode)
{
  switch (keyCode)
    {
      case WXK_ADD:
      case WXK_NUMPAD_ADD:
        if (drawTreshold<1.0)
        {
          drawTreshold+=0.1;
          updated=true;
        }
        dPrint(1,"treshold set to %f",drawTreshold);
        break;
      case WXK_SUBTRACT:
      case WXK_NUMPAD_SUBTRACT:
        if (drawTreshold>=0.1)
        {
          drawTreshold-=0.1;
          updated=true;
        }
        dPrint(1,"treshold set to %f",drawTreshold);
        break;
    }
}

void CMapLayer::OnKeyUp(int keyCode)
{
  
}

void CMapLayer::OnKillFocus()
{
  
}

void CMapLayer::Update(int timeout)
{
  MaCI::Map::CMapClient *mc=(MaCI::Map::CMapClient*)MaCIClient;
    
  //MaCI::Map::CMapData *md=new MaCI::Map::CMapData;
  MaCI::Map::CMapData md;
  
  
  if (mc!=NULL)
  {
    if (!inited)
    {
      MaCI::Map::CMapData map;
      dPrint(1,"Trying to get current map from server...");
      if (mc->GetMap(map,2000))
      {
        
        dPrint(1,"Received current map from server.");
        /*
        hmMapper.SetType(MaCI::Map::KGridTypeHeightMap);
        hmMapper.SetResolution(map.GetMapHeader()->gridDataResolution);
        hmMapper.Update(map);
        occuMapper.SetType(MaCI::Map::KGridTypeOccupancy);
        occuMapper.SetResolution(map.GetMapHeader()->gridDataResolution);
        occuMapper.Update(map);
        */
        mapper.Update(map,groundUpdates,occuUpdates);
        
        //not bothering to do anything with these right now..
        groundUpdates.clear();
        occuUpdates.clear();
        
        inited=true;
        updated=true;
      } else {
        dPrint(2,"Failed to get map from server.");
      }
    }
    
    while (mc->GetMapUpdateEvent(md,&seq,0))
    {
      //dPrint(1,"received map data!");
      if (md.GetMapUpdateHeader())
      {
        Lock();
        updated=true;
        /*if (md->GetMapHeader()->clear)
        {
          //clear old map data
          std::vector<MaCI::Map::CMapData*>::iterator iter;
          for (iter=mapData.begin();iter!=mapData.end();iter++)
            delete *iter;
          mapData.clear();
        }*/
        //mapData.push_back(md);
        
        //hmMapper.Update(md);
        //occuMapper.Update(md);
        mapper.Update(md,groundUpdates,occuUpdates);
        
        Unlock();
      }
      md.Reset();
    }
  }
  //delete md;

}

void CMapLayer::GLTransform()
{
  
}

float CMapLayer::GetX()
{
  return 0;
}

float CMapLayer::GetY()
{
  return 0;
}


/**** CAlarmLayer  ****/

CAlarmLayer::CAlarmLayer(std::string Name, CMachineUnit* unit, MaCI::Alarm::CAlarmClient* AlarmClient)
  :CDeviceLayer(Name,unit,AlarmClient)
{
  Enabled=true;
  alarmArrayMutex=ownMutex_Init();
}

CAlarmLayer::~CAlarmLayer()
{
  
}

void CAlarmLayer::Draw(float viewLeft, float viewRight, float viewTop, float viewBottom, float scale)//GLuint renderMode, float alpha);
{
  glPushMatrix();
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  ownMutex_Lock(alarmArrayMutex);
  
  if (alarmArray.size())
  {
    std::list<alarm>::iterator i;
    
    char c[2]={'!',0};
    
    float step=2.0*M_PI/8.0;
    float angle=M_PI/2.0-(alarmArray.size()-1)*step/2.0;
    
    
    float x=0;
    float y=0;
    
    if (refLayer)
    {
      x=refLayer->GetX();
      y=refLayer->GetY();
    }
    
    for (i=alarmArray.begin();i!=alarmArray.end();i++)
    {
      glPushMatrix();
      
      if (i->event.type==MaCI::Alarm::KEventTypeQuestion)
      {
        glColor3f(1,1,1);
        c[0]='?';
      }else if (i->event.type==MaCI::Alarm::KEventTypeWarning || 
                i->event.type==MaCI::Alarm::KEventTypeError) {
        glColor3f(1,0.6,0.6);
        c[0]='!';
      } else {
        glColor3f(1,1,1);
        c[0]='!';
      }
      
      
      glTranslatef(x,y,0);
      
      glBegin(GL_TRIANGLES);
      
      glVertex2f(cos(angle+0.05)*BUBBLE_D/2.0,sin(angle+0.05)*BUBBLE_D/2.0);

      glVertex2f(cos(angle+0.05)*BUBBLE_D,sin(angle+0.05)*BUBBLE_D);
      glVertex2f(cos(angle-0.05)*BUBBLE_D,sin(angle-0.05)*BUBBLE_D);
      
      glEnd();
      
      glTranslatef(cos(angle)*BUBBLE_D,sin(angle)*BUBBLE_D,0);
      
      glBegin(GL_QUADS);
      
      glVertex2f(-BUBBLE_W, -BUBBLE_W);
      glVertex2f(-BUBBLE_W, BUBBLE_W);
      glVertex2f(BUBBLE_W, BUBBLE_W);
      glVertex2f(BUBBLE_W, -BUBBLE_W);
      
      glEnd();
      glColor3f(0,0,0);
      
      drawText(-3.0/scale,-8.0/scale,GLUT_BITMAP_HELVETICA_18,c);
      
      angle=angle+step;
      glPopMatrix();
    }
    
  }
  ownMutex_Unlock(alarmArrayMutex);
  glPopMatrix();
  glPopAttrib();
}

void CAlarmLayer::OnClick(float x, float y)
{

};

void CAlarmLayer::OnKey(int keyCode)
{

}

void CAlarmLayer::OnKeyUp(int keyCode)
{
}

void CAlarmLayer::OnKillFocus()
{
  
}

void CAlarmLayer::Update(int timeout)
{
  MaCI::Alarm::CAlarmClient *ac=(MaCI::Alarm::CAlarmClient*)MaCIClient;
  MaCI::Alarm::CAlarmData ad;
  ownMutex_Lock(alarmArrayMutex);
  
  //get new events
  if (ac->GetAlarmEvent(ad, timeout ) )
  {
    if (ad.GetAlarmEvent())
    {
      alarm a;
      a.event=*(ad.GetAlarmEvent());
      if (ad.GetTimestamp() )
      {
        a.time=*(ad.GetTimestamp());
      } else {
        MaCI::Common::TTimestamp t;
        t.SetToCurrentTime();
        a.time=t;
      }
      //dPrint(1,"alarm added to array");
      alarmArray.push_back(a);
    }
  } else {
    //ownSleep_ms(10);
  }
  
  //delete ones that are too old
  gim::time t(true);
  t.reduceTimeMs(ALARM_LIFETIME_S*1000);

  while (alarmArray.size())
  {
    //if too old remove
    if (alarmArray.front().time.GetGimTime() < t)
    {
      //dPrint(1,"alarm removed from array");
      alarmArray.pop_front();
    } else {
      //if no too old alarms, leave loop
      break;
    }
  }
  
  ownMutex_Unlock(alarmArrayMutex);
}

void CAlarmLayer::GLTransform()
{

}

float CAlarmLayer::GetX()
{
  return 0;
}

float CAlarmLayer::GetY()
{
  return 0;
}

/**** CEnergyLayer  ****/

CEnergyLayer::CEnergyLayer(std::string Name, CMachineUnit* unit, MaCI::Energy::CEnergyClient* EnergyClient)
  :CDeviceLayer(Name,unit,EnergyClient),
  seq(0),
  working(false),
  level(0), maxLevel(0), consumption(0)
{
  Enabled=true;
}

CEnergyLayer::~CEnergyLayer()
{
  
}

void CEnergyLayer::Draw(float viewLeft, float viewRight, float viewTop, float viewBottom, float scale)//GLuint renderMode, float alpha);
{
  if (!working)
    return;
  
  glPushMatrix();
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  
  float x=0;
  float y=0;
  
  if (refLayer)
  {
    x=refLayer->GetX();
    y=refLayer->GetY();
  }
  
  glTranslatef(x-ICON_W/2.0,y-0.3-ICON_H,0);
  
  glBegin(GL_LINE_LOOP);
  
  glColor3f(0.7, 0.7, 0.7);
  glVertex2f(0,0);
  glVertex2f(0,ICON_H);
  glVertex2f(ICON_W,ICON_H);
  glVertex2f(ICON_W,0.0);
  
  glEnd();
  
  glBegin(GL_QUADS);
  
  glVertex2f(ICON_W,ICON_H/3.0);
  glVertex2f(ICON_W,ICON_H/3.0*2.0);
  glVertex2f(ICON_W*1.05,ICON_H/3.0*2.0);
  glVertex2f(ICON_W*1.05,ICON_H/3.0);
  
  glColor3f(1-level*level, 1-(1.0-level)*(1.0-level), 0);
  glVertex2f(0.0, 0.0);
  glVertex2f(0.0, ICON_H);
  glVertex2f(ICON_W*level, ICON_H);
  glVertex2f(ICON_W*level, 0);
  
  glEnd();
  
  glPopMatrix();
  glPopAttrib();
}

void CEnergyLayer::OnClick(float x, float y)
{

}

void CEnergyLayer::OnKey(int keyCode)
{

}

void CEnergyLayer::OnKeyUp(int keyCode)
{
  
}

void CEnergyLayer::OnKillFocus()
{
  
}

void CEnergyLayer::Update(int timeout)
{
  MaCI::Energy::CEnergyClient *ec=(MaCI::Energy::CEnergyClient*)MaCIClient;
  MaCI::Energy::CEnergyData ed;
  
  //get new events
  if (ec->GetEnergyEvent(ed, &seq, timeout ) )
  {
    if (ed.GetEnergyEvent() && ed.GetEnergyEvent()->type==MaCI::Energy::KEventTypeEnergyData )
    {
      
      working=true;
      level=ed.GetEnergyEvent()->energyLevel/100.0;
      maxLevel=ed.GetEnergyEvent()->energyMax;
      consumption=ed.GetEnergyEvent()->consumption;
    }
  }
}

void CEnergyLayer::GLTransform()
{

}

float CEnergyLayer::GetX()
{
  return 0;
}

float CEnergyLayer::GetY()
{
  return 0;
}


